function new_state = f_fun(model, state, V, U1)

[dim,nov] = size(state);
new_state = zeros(dim,nov);
dimX  = model.statedim;
dimV  = model.Vdim;

for k=1:nov,
    new_state(1:dimX,k) = ffun(model, state(1:dimX,k), V(1:dimV,k), U1);
end



function new_state = ffun(~, state, V, U1)


new_state = ukfRotateQuat(state,V,U1,0.022488);






function out = ukfRotateQuat(in, noise, rate, dt)

out = zeros(7,1);
q0 = in(1);
q1 = in(2);
q2 = in(3);
q3 = in(4);

out(5) = in(5) + noise(4)*dt;
out(6) = in(6) + noise(5)*dt;
out(7) = in(7) + noise(6)*dt;

wx = (rate(1)+in(5)+noise(1)) * dt;
wy = (rate(2)+in(6)+noise(2)) * dt;
wz = (rate(3)+in(7)+noise(3)) * dt;


s = sqrt(wx^2 + wy^2 + wz^2)*0.5;
t = -(0.5 * sin(s) / s);

if s == 0
    t = 0;
end

wx = wx*t;
wy = wy*t;
wz = wz*t;

qMag = q0^2 + q1^2 + q2^2 + q3^2;
lg = cos(s) + (1-qMag^2) * dt * dt;

out(1) =  lg * q0 + wx * q1 + wy * q2 + wz * q3;
out(2) = -wx * q0 + lg * q1 - wz * q2 + wy * q3;
out(3) = -wy * q0 + wz * q1 + lg * q2 - wx * q3;
out(4) = -wz * q0 - wy * q1 + wx * q2 + lg * q3;


%out(1:4) = out(1:4)/norm(out(1:4));
